/*
 * utility.c
 *
 *  Created on: 8 Nov 2017
 *      Author: Standa
 */

#include "utility.h"
#include <sys/types.h>
#include <math.h>
#include "qfplib.h"
#include "qfpio.h"


/*
 * Calculates mean and stdev based on stream of values (online)
 * https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance - Welford's online algorithm
 * Using __ieee754_sqrtf instead sqrtf because it does not link huge ddiv function
 */
void ut_CalculateOnlineMeanStdev(float newValue, ut_MeanStdevTd* result){
	if(result){
		result->Cnt++;													//Increment sample count
		float Delta1 = qfp_fsub(newValue, result->Mean);
		result->Mean = qfp_fadd(result->Mean, qfp_fdiv( Delta1, result->Cnt) );							//Calculate mean
		result->M2 = qfp_fadd( result->M2, qfp_fmul( Delta1, qfp_fsub(newValue, result->Mean) ) );				//Calculate M2
		result->Stdev = qfp_fsqrt( qfp_fdiv( result->M2, result->Cnt ) );
	}
}


/*
 * Initializes/resets the storage for mean and stdev results and calculation parameters
 */
void ut_InitializeOnlineMeanStdev(ut_MeanStdevTd* result){
	if(result) for(uint8_t i=0; i<sizeof(ut_MeanStdevTd); i++) ((uint8_t*)result)[i] = 0;
}


/*
 * @brief: Converts value in degrees to radians
 */
inline float ut_DegToRad(float deg){
	return qfp_fmul(deg, 0.0174532925);		//2*PI*1RADIAN = 360deg
}


/*
 * @brief: Converts value in radians to degrees
 */
//Converts value in radians to degrees
inline float ut_RadToDeg(float rad){
	return qfp_fmul(rad, 57.2957795);		//2*PI*1RADIAN = 360deg
}


/*
 * Converts specified float val into string dest of maximum length maxLen
 * x +-1E6, decimal numbers decCnt from 0 to 3
 * user must make sure that the number will fit into buffer and buffer must be two chars longer
 */
static char Prefixes[] = {'G', 'M', 'K', '.', 'm', 'u', 'n'};
static float Scales[] = {1E9, 1E6, 1E3, 1, 1E-3, 1E-6, 1E-9};

uint16_t ut_ConvFloatToEngStr(float val, char *dest, char *temp, uint8_t maxLen, uint8_t decCnt){
	float ScaledVal;
	uint8_t CharCntr = 0;
    char *s = temp; 														//go to end of buffer
    uint16_t decimals, units;  												//variable to store the decimals, variable to store the units (part to left of decimal place)

	if(UT_ISNAN(val)) return 0;												//Check NAN
	if(val >= 1E12 && val <= -1E12) return 0;								//Check range

	uint8_t i;
	for(i=0; i<UT_SIZEOFARRAY(Prefixes); i++){
		ScaledVal = qfp_fdiv(val, Scales[i]);
		if( qfp_float2int(ScaledVal) >= 1000 || qfp_float2int(ScaledVal) <= -1000 ) break;
	}

	if(i==0) return 0;														//Out of upper range
	ScaledVal = qfp_fdiv(val, Scales[i-1]);


    if(ScaledVal < 0){ 														//Take care of negative numbers
        decimals = qfp_float2uint( qfp_fmul(ScaledVal, -1000) ) % 1000; 	//Make 1000 for 3 decimals etc.
        units = qfp_float2int( qfp_fmul(-1, ScaledVal));
    }
    else{ 																	//Positive numbers
        decimals = qfp_float2uint(qfp_fmul(ScaledVal, 1000)) % 1000;
        units = qfp_float2int(ScaledVal);
    }


	while(decCnt!=0){														//Place all decimal places
		*s++ = (decimals % 10) + '0';
		decimals /= 10; 										//Repeat for as many decimal places as you need
		decCnt--;
		CharCntr++;
	}

	*s++ = Prefixes[i-1];										//Place scaling unit instead of decimal dot
	CharCntr++;

    while (units > 0) {
        *s++ = (units % 10) + '0';
        units /= 10;
        CharCntr++;
    }

    if (ScaledVal < 0){
    	*s++ = '-'; 											//Unary minus sign for negative numbers
    	CharCntr++;
    }

    for(i=0; i<maxLen; i++){
    	if(i==CharCntr){
    		break;
    	}
    	dest[i] = temp[CharCntr-i-1];
    }

    return i;
}






/*
 * Reset complex number c to zero
 */
err_Td ut_CplxReset(ut_CplxTd *v){
	if(v){
		for(uint8_t i=0; i<sizeof(ut_CplxTd); i++) ((uint8_t*)v)[i] = 0;
		return err_Td_Ok;
	}
	else{
		return err_Td_Null;
	}
}


err_Td ut_CplxInitByRI(ut_CplxTd *c, float real, float imaginary){
	if(c){
		c->Real = real;
		c->Imaginary = imaginary;
		c->Magnitude = qfp_fsqrt( qfp_fadd( qfp_fmul(real, real), qfp_fmul(imaginary, imaginary) ) );	//M=sqrt(x*x+y*y)
		c->Angle = qfp_fatan2(imaginary, real);
		return err_Td_Ok;
	}
	else{
		return err_Td_Null;
	}
}


err_Td ut_CplxInitByMA(ut_CplxTd *c, float magnitude, float angle){
	if(c){

		c->Magnitude = magnitude;
		c->Angle = angle;
		c->Real = qfp_fmul(magnitude, qfp_fcos(angle));			//X = Mag * cos(Fi);
		c->Imaginary = qfp_fmul(magnitude, qfp_fsin(angle));	//Y = Mag * sin(Fi);
		return err_Td_Ok;
	}
	else{
		return err_Td_Null;
	}
}


err_Td ut_CplxAdd(ut_CplxTd *cResult, ut_CplxTd *c1, ut_CplxTd *c2){
	return err_Td_NotImple;
}


/*
 * cResult = c1 - c2;
 */
err_Td ut_CplxSub(ut_CplxTd *cResult, ut_CplxTd *c1, ut_CplxTd *c2){
	if(c1 && c2 && cResult){
		return ut_CplxInitByRI(cResult, qfp_fsub(c1->Real, c2->Real), qfp_fsub(c1->Imaginary, c2->Imaginary));
	}
	else{
		return err_Td_Null;
	}
}


/*
 * cResult = c1 * c2;
 */
err_Td ut_CplxMul(ut_CplxTd *cResult, ut_CplxTd *c1, ut_CplxTd *c2){
	if(c1 && c2 && cResult){
		return ut_CplxInitByMA(cResult, qfp_fmul(c1->Magnitude, c2->Magnitude), qfp_fadd(c1->Angle, c2->Angle));
	}
	else{
		return err_Td_Null;
	}
}


/*
 * cResult = c1 / c2;
 */
err_Td ut_CplxDiv(ut_CplxTd *cResult, ut_CplxTd *c1, ut_CplxTd *c2){
	if(c1 && c2 && cResult){
		return ut_CplxInitByMA(cResult, qfp_fdiv(c1->Magnitude, c2->Magnitude), qfp_fsub(c1->Angle, c2->Angle));
	}
	else{
		return err_Td_Null;
	}
}


/*
 *
 */
err_Td ut_CplxScaleM(ut_CplxTd *c, float magScale){
	if(c){

		c->Magnitude = qfp_fmul(c->Magnitude, magScale);
		c->Real = qfp_fmul(c->Real, magScale);
		c->Imaginary = qfp_fmul(c->Imaginary, magScale);
		return err_Td_Ok;
	}
	else{
		return err_Td_Null;
	}
}
